import lejos.nxt.LCD;
import lejos.robotics.subsumption.Behavior;

/**
 * Trouve la ligne gauche (gris foncé chemin à suivre, couloir de gauche indiqué par le gris clair si sens inverse, sinon gris intermédiaire)
 * @author Nicolas
 */
public class TrouverLigneAGauche implements Behavior {

    /**
     * Remet le robot sur le droit chemin si ligne gauche trouvée
     * @return true si le robot est dans le gris clair+sens normal ou gris foncé+sens inversé
     */
    public boolean takeControl() {
        try {
            //il est alors trop à droite par rapport à la ligne=>on va le faire tourner vers la gauche
            int lightValue = Robot.readLightValue();
            return (Robot.GRIS_CLAIR.contient(lightValue) && Robot.estEnSensNormal()) || (Robot.GRIS_FONCE.contient(lightValue) && !Robot.estEnSensNormal());

        } catch (Exception ex) {
            Robot.justSay("Erreur takeControl TrouverLigneAGauche");
            return false;
        }
    }
	
    public void suppress() {
        //TODO : On arrête de tourner
        try {
        } catch (Exception ex) {
            Robot.justSay("Erreur suppress TrouverLigneAGauche");
        }
    }

    /**
     * fait tourner le robot vers la gauche
     */
    public void action() {
        try {
            //LCD.clear();
            LCD.drawString("Trouver gauche !", 0, 0);
            LCD.refresh();   
            //Tant qu'on a pas trouvé la ligne, on tourne par pas de -1°
            int pas=2;
            float pas_steer=4f;
            //Si le robot est en sens inverse, on inverse l'angle
            if(!Robot.estEnSensNormal()) pas=-pas;    
            while((Robot.GRIS_CLAIR.contient(Robot.readLightValue())&& Robot.estEnSensNormal())
                    || (Robot.GRIS_FONCE.contient(Robot.readLightValue()) && ! Robot.estEnSensNormal())){
                LCD.drawString("valeur"+Robot.readLightValue(), 0, 1);
                LCD.refresh();
                Robot.Pilot.forward();
                //arc vers la droite avec roue droite bloquée
                if(!Robot.estEnSensNormal()){
                    //Robot.Pilot.steer(-100, pas_steer);
                }else{
                    //Robot.Pilot.steer(pas_steer, pas_steer, true);
                    //Robot.Pilot.steer(100, pas_steer);
                }
                Robot.Pilot.rotate(pas, false);
            }
        } catch (Exception ex) {
            Robot.justSay("Erreur action TrouverLigneAGauche");
        }
    }
}
